function [ epsilonB ] = gyroerror_simp( DalphaBl, domegaIBBiasS, dKScalP, dKScalN, dPSS0, fB, DGSens, ignore2Od ) %#codegen
%GYROERROR_SIMP 计算陀螺器件误差引入的测量误差
%
% Tips:
% 1. 对增量计算，传入的domegaIBBiasS需要乘以采样周期
%
% Input Arguments:
% DalphaBl: m*3矩阵，m为采样个数，角速度，单位rad/s
% domegaIBBiasS: m*3矩阵，零偏，单位rad/s，默认全为0
% dKScalP: m*3矩阵，正向标度因数误差，无单位，默认全为0
% dKScalN: m*3矩阵，负向标度因数误差，无单位，默认全为0
% dPSS0: 3*3*m矩阵，失准角（安装误差），无单位，默认全为0
% fB: m*3矩阵，比力，单位m/s^2，默认全为0
% DGSens: 3*3*m矩阵，g敏感项系数，(rad/s)/(m/s^2)，默认全为0
% ignore2Od: 逻辑标量，true表示忽略2阶误差，默认为true
%
% Output Arguments:
% epsilonB: m*3矩阵，角速度误差，单位rad/s
%
% References:
% [1] 理论文档“惯组误差模型”第2节

m = size(DalphaBl, 1);
if nargin < 2
    domegaIBBiasS = zeros(m, 3);
end
if nargin < 3
    dKScalP = zeros(m, 3);
end
if nargin < 4
    dKScalN = [];
end
if nargin < 5
    dPSS0 = zeros(3, 3, m);
end
if nargin < 6
    fB = zeros(m, 3);
end
if nargin < 7
    DGSens = zeros(3, 3, m);
end
if nargin < 8
    ignore2Od = true;
end

dSF = dKScalP;
if ~isempty(dKScalN)
    for i=1:m
        for j=1:3
            if DalphaBl(i, j) < 0
                dSF(i, j) = dKScalN(i, j);
            end
        end
    end
end
dSFMAErr = coder.nullcopy(NaN(m, 3));
GSErr =  coder.nullcopy(NaN(m, 3));
for i=1:m
    dSFMAErr(i, :) = DalphaBl(i, :) * (diag(dSF(i, :))+dPSS0(:, :, i)')'; % MA
    GSErr(i, :) = fB(i, :) * DGSens(:, :, i)';
end
epsilonB1 = domegaIBBiasS + dSFMAErr + GSErr;

if ignore2Od
    epsilonB2 = zeros(m, 3);
else
    for i=1:m
        dSFMAErr(i, :) = DalphaBl(i, :) * dPSS0(:, :, i); % MA'
        GSErr(i, :) = fB(i, :) * DGSens(:, :, i)';
    end
    tmp = domegaIBBiasS + dSFMAErr + GSErr;
    dSF = dKScalP;
    if ~isempty(dKScalN)
        for i=1:m
            for j=1:3
                if tmp(i, j) < 0
                    dSF(i, j) = dKScalN(i, j);
                end
            end
        end
    end
    epsilonB2 = dSF .* tmp;
end

epsilonB = epsilonB1 + epsilonB2;
end